Advanced Lane Finding Project

The goals / steps of this project are the following:

  • Compute the camera calibration matrix and distortion coefficients given a set of chessboard images.
  • Apply a distortion correction to raw images.
  • Use color transforms, gradients, etc., to create a thresholded binary image.
  • Apply a perspective transform to rectify binary image ("birds-eye view").
  • Detect lane pixels and fit to find the lane boundary.
  • Determine the curvature of the lane and vehicle position with respect to center.
  • Warp the detected lane boundaries back onto the original image.
  • Output visual display of the lane boundaries and numerical estimation of lane curvature and vehicle position.

Preperation and utilities

In [1]:
FLG_DEBUG = False#True#False #True
In [2]:
import numpy as np
import cv2
import glob
import matplotlib.pyplot as plt
import matplotlib.image as mpimg
%matplotlib inline
In [3]:
def show_images(images):

    num_img = len(images)
    #print(num_img)
    #plt.figure(figsize=(15,5))
    #fig, axes = plt.subplots(num_img, 1)
    #for axe, i in zip(axes.flat, range(num_img)):
    #    print(i)
    #    axe.imshow(images[i])
    #    #plt.imshow(images[i])
    for i in range(num_img):
        #print("i:",i)
        plt.figure(i)
        #plt.subplot(num_img, 1, i+1)
        
        plt.imshow(images[i])
        #plt.imshow(images[i])

#plt.show()
In [4]:
def show_two_images(img1, img2, title1="Original Image", title2="Result Image"):

    f, (ax1, ax2) = plt.subplots(1, 2, figsize=(24, 9))
    f.tight_layout()
    ax1.imshow(img1)
    ax1.set_title(title1, fontsize=50)
    ax2.imshow(img2)
    ax2.set_title(title2, fontsize=50)
    plt.subplots_adjust(left=0., right=1, top=0.9, bottom=0.)
In [5]:
def show_pair_images(images1, images2):
    for i in range(len(images1)):
        show_two_images(images1[i], imges2[i])

Camera calibration

Function Definition

In [6]:
def cal_undistort(img, objpoints, imgpoints):
    """
    a function that takes an image, object points, and image points
    performs the camera calibration, image distortion correction and 
    returns the undistorted image
    """
    gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
    ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
    img_dst = cv2.undistort(img, mtx, dist, None, mtx)
    return img_dst
In [7]:
def compute_camera_calibration(images,chessboard_size_x=9, chessboard_size_y=6):
    # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
    objp = np.zeros((chessboard_size_y*chessboard_size_x,3), np.float32)
    objp[:,:2] = np.mgrid[0:chessboard_size_x,0:chessboard_size_y].T.reshape(-1,2)

    # Arrays to store object points and image points from all the images.
    objpoints = [] # 3d points in real world space
    imgpoints = [] # 2d points in image plane.

    # Make a list of calibration images
    images_converted = []

    # Step through the list and search for chessboard corners
    for fname in images:
        #print(fname)
        img = cv2.imread(fname)
        img_converted = np.copy(img)
        gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)

        # Find the chessboard corners
        ret, corners = cv2.findChessboardCorners(gray, (chessboard_size_x,chessboard_size_y),None)

        # If found, add object points, image points
        if ret == True:
            objpoints.append(objp)
            imgpoints.append(corners)

            # Draw and display the corners
            img_converted = cv2.drawChessboardCorners(img_converted, (chessboard_size_x,chessboard_size_y), corners, ret)
            images_converted.append(img_converted)
            
            # Undistort the original image using objpoints and imgpoints
            img_undistorted = cal_undistort(img, objpoints, imgpoints)
            show_two_images(img_converted, img_undistorted)

    return objpoints, imgpoints

Correcting for Distortion

I used chessboard images to obtain image points and object points, and then compute the calibration and undistortion.

In [8]:
camera_images = glob.glob('./camera_cal/calibration*.jpg')
objpoints, imgpoints = compute_camera_calibration(camera_images)
In [9]:
test_img = cv2.imread('./camera_cal/calibration2.jpg')

test_img_undistorted = cal_undistort(test_img, objpoints, imgpoints)
show_two_images(test_img, test_img_undistorted, title2="Undistorted Image")
In [10]:
test_image = mpimg.imread('./test_images/straight_lines1.jpg')
test_image_undistorted = cal_undistort(test_image, objpoints, imgpoints)
show_two_images(test_image, test_image_undistorted, title2="Undistorted Image")

Thresholded Binary Image Creation

Function Definition

In [11]:
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.image as mpimg


# Edit this function to create your own pipeline.
def pipeline_thresh(img, s_thresh=(170, 255), sx_thresh=(20, 100)):
    img = np.copy(img)
    # Convert to HLS color space and separate the V channel
    hls = cv2.cvtColor(img, cv2.COLOR_RGB2HLS).astype(np.float)
    l_channel = hls[:,:,1]
    s_channel = hls[:,:,2]
    
    # Grayscale image
    # NOTE: we already saw that standard grayscaling lost color information for the lane lines
    # Explore gradients in other colors spaces / color channels to see what might work better
    gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)

    # Sobel x
    #sobelx = cv2.Sobel(l_channel, cv2.CV_64F, 1, 0) # Take the derivative in x
    sobelx = cv2.Sobel(gray, cv2.CV_64F, 1, 0) # Take the derivative in x
    
    abs_sobelx = np.absolute(sobelx) # Absolute x derivative to accentuate lines away from horizontal
    scaled_sobel = np.uint8(255*abs_sobelx/np.max(abs_sobelx))
    
    # Threshold x gradient
    sxbinary = np.zeros_like(scaled_sobel)
    sxbinary[(scaled_sobel >= sx_thresh[0]) & (scaled_sobel <= sx_thresh[1])] = 1
    
    # Threshold color channel
    s_binary = np.zeros_like(s_channel)
    s_binary[(s_channel >= s_thresh[0]) & (s_channel <= s_thresh[1])] = 1
    # Stack each channel
    # Note color_binary[:, :, 0] is all 0s, effectively an all black image. It might
    # be beneficial to replace this channel with something else.
    color_binary = np.dstack(( np.zeros_like(sxbinary), sxbinary, s_binary)) * 255
    
    # Combine the two binary thresholds
    combined_binary = np.zeros_like(sxbinary)
    combined_binary[(s_binary == 1) | (sxbinary == 1)] = 1

    return combined_binary

Sample Execution

In [12]:
test_image = mpimg.imread('./test_images/straight_lines1.jpg')

test_image_threshed = pipeline_thresh(test_image)

show_two_images(test_image, test_image_threshed, title2="Thresholded Binary Image")

I checked the pipeline using the following image from the challenge video.

In [13]:
test_img = mpimg.imread('./issue_images/org_video_image_20171217_10_22_06.jpg')
test_output = pipeline_thresh(test_img, s_thresh=(170, 255), sx_thresh=(5, 100))
show_two_images(test_img, test_output, title2="Thresholded Binary Image")

Perspective Transformation

Function Definition

In [14]:
"""
Y_TOP = 435#440
Y_BTM = 668

X_CTR = 640#640 = 1280/2

##X_TOP_WDT = 48#45#50#55#65#50#50#60#60
X_BTM_WDT = 1000#1280#1000#1000#900#850

# less space is used
Y_TOP = 450#435#440
X_TOP_WDT = 120#48#45#50#55#65#50#50#60#60


TOP_LEFT = (X_CTR - X_TOP_WDT/2 , Y_TOP)
TOP_RIGHT = (X_CTR + X_TOP_WDT/2, Y_TOP)
BTM_LEFT = (X_CTR - X_BTM_WDT/2 , Y_BTM)
BTM_RIGHT = (X_CTR + X_BTM_WDT/2, Y_BTM)

SRC_TRAPEZOID = [TOP_LEFT, TOP_RIGHT, BTM_RIGHT, BTM_LEFT]
"""
Out[14]:
'\nY_TOP = 435#440\nY_BTM = 668\n\nX_CTR = 640#640 = 1280/2\n\n##X_TOP_WDT = 48#45#50#55#65#50#50#60#60\nX_BTM_WDT = 1000#1280#1000#1000#900#850\n\n# less space is used\nY_TOP = 450#435#440\nX_TOP_WDT = 120#48#45#50#55#65#50#50#60#60\n\n\nTOP_LEFT = (X_CTR - X_TOP_WDT/2 , Y_TOP)\nTOP_RIGHT = (X_CTR + X_TOP_WDT/2, Y_TOP)\nBTM_LEFT = (X_CTR - X_BTM_WDT/2 , Y_BTM)\nBTM_RIGHT = (X_CTR + X_BTM_WDT/2, Y_BTM)\n\nSRC_TRAPEZOID = [TOP_LEFT, TOP_RIGHT, BTM_RIGHT, BTM_LEFT]\n'
In [15]:
#Y_TOP = 450#435#440
Y_BTM = 668

X_CTR = 640#640 = 1280/2

X_TOP_WDT = 120#48#45#50#55#65#50#50#60#60
X_BTM_WDT = 1000#1280#1000#1000#900#850

Y_TOP = 460#435#440
X_TOP_WDT = 150#48#45#50#55#65#50#50#60#60

#X_BTM_WDT = 900

TOP_LEFT = (X_CTR - X_TOP_WDT/2 , Y_TOP)
TOP_RIGHT = (X_CTR + X_TOP_WDT/2, Y_TOP)
BTM_LEFT = (X_CTR - X_BTM_WDT/2 , Y_BTM)
BTM_RIGHT = (X_CTR + X_BTM_WDT/2, Y_BTM)

SRC_TRAPEZOID = [TOP_LEFT, TOP_RIGHT, BTM_RIGHT, BTM_LEFT]
In [16]:
def pipeline_warp(image, offsetX = 50, offsetY = 0):
    offset = 100 # offset for dst points
    img_size = (image.shape[1], image.shape[0])
    w = img_size[1]
    h = img_size[1]
    #offsetX = 30 #
    
    src = np.float32(SRC_TRAPEZOID)

    dst = np.float32([[offsetX, offsetY], [w - offsetX, offsetY], 
                      [w - offsetX, h - offsetY],
                      [offsetX, h - offsetY]])
    if FLG_DEBUG == True:
        print("src",src)
        print("dst",dst)
        
    M = cv2.getPerspectiveTransform(src, dst)
    
    warped = cv2.warpPerspective(image,M,(h, w))
    
    Minv = cv2.getPerspectiveTransform(dst, src)

    return warped, M, Minv, dst

Sample Execution

In [17]:
test_img = mpimg.imread('./test_images/test1.jpg')
test_image_threshed = pipeline_thresh(test_img)
test_image_warped, M, Minv, _ = pipeline_warp(test_image_threshed)
show_two_images(test_img, test_image_threshed, title2="Binary Image")
show_two_images(test_img, test_image_warped, title2="Warped Image")

Finding the Lines

Class definition

In [18]:
#SPEC_CAMERA_IMG_HEIGHT = 720?
#SPEC_CAMERA_IMG_WIDTH = 1300?
SPEC_WARP_IMG_HEIGHT = 720
SPEC_WARP_IMG_WIDTH = 720
SPEC_WARP_IMG_X_MID = 360
#U.S. regulations that require a minimum lane width of 12 feet or 3.7 meters
LANE_WIDTH_IN_METER = 3.7
OBSERVED_LANE_PIX_IN_WARP_IMG = 500
OBSERVED_METER_PER_PIXEL = LANE_WIDTH_IN_METER/OBSERVED_LANE_PIX_IN_WARP_IMG
In [19]:
def find_vehicle_position(left_fit, right_fit, y_btm=SPEC_WARP_IMG_HEIGHT, image_mid=SPEC_WARP_IMG_X_MID):

    x_left = left_fit[0]*y_btm**2 + left_fit[1]*y_btm + left_fit[2]
    x_right = right_fit[0]*y_btm**2 + right_fit[1]*y_btm + right_fit[2]
    
    #print("x_left", x_left)
    #print("x_right", x_right)
    
    lane_width = x_right - x_left

    x_mid = x_left + (lane_width)//2
    
    #print("x_mid", x_mid)
    
    vehicle_position = image_mid - x_mid

    vehicle_position_meter = vehicle_position*OBSERVED_METER_PER_PIXEL
    
    return vehicle_position_meter#vehicle_position
In [20]:
def calc_radius_of_curvature(fit_cr, y_eval=SPEC_WARP_IMG_HEIGHT-1):
    # Define conversions in x and y from pixels space to meters
    #ym_per_pix = 30/720 # meters per pixel in y dimension
    #xm_per_pix = 3.7/700 # meters per pixel in x dimension
    
    #ym_per_pix = 30/720 # meters per pixel in y dimension
    xm_per_pix = LANE_WIDTH_IN_METER/OBSERVED_LANE_PIX_IN_WARP_IMG # meters per pixel in x dimension
    ym_per_pix = xm_per_pix # warped image has the same y pix as x pix
    
    # Calculate the new radii of curvature
    if fit_cr[0] == 0:
        radius = 0
    else:
        radius = ((1 + (2*fit_cr[0]*y_eval*ym_per_pix + fit_cr[1])**2)**1.5) / np.absolute(2*fit_cr[0])
    # Now our radius of curvature is in meters
    return radius
In [21]:
# Define a class to receive the characteristics of each line detection
class Line():

    def __init__(self, name=""):
        self.__LAST_N_ITERATIONS = 3#5

        self.__FIT_OUTLIER_0 = 0.003#1
        self.__FIT_OUTLIER_1 = 0.1#0.3#0.5
        self.__FIT_OUTLIER_2 = 8#10
        
        self.name = name
        # was the line detected in the last iteration?
        self.detected = False  
        # x values of the last n fits of the line
        self.recent_xfitted = [] 
        
        #average x values of the fitted line over the last n iterations
        self.bestx = None     
        #polynomial coefficients averaged over the last n iterations
        self.best_fit = None 
        
        #polynomial coefficients for the most recent fit
        self.current_fit = np.array([0,0,0], dtype='float')  # Not sure why [np.array([False])]  
        
        self.prev_fit = np.array([0,0,0], dtype='float') 
        self.outlier_fit = np.array([0,0,0], dtype='float') 
        
        #radius of curvature of the line in some units
        self.radius_of_curvature = None 
        #distance in meters of vehicle center from the line
        self.line_base_pos = None 
        
        #difference in fit coefficients between last and new fits
        self.diffs = np.array([0,0,0], dtype='float') 
        
        #x values for detected line pixels
        self.allx = None  
        #y values for detected line pixels
        self.ally = None
        
    def calc_average_x_values_of_fitted_line(self):
        #average x values of the fitted line over the last n iterations
        if len(self.recent_xfitted) > self.__LAST_N_ITERATIONS:
            self.recent_xfitted = self.recent_xfitted[-self.__LAST_N_ITERATIONS:]
        self.bestx = np.array(self.recent_xfitted).mean(axis=0).tolist()  

    def update_averaged_polynomial_coefficients(self):
        #polynomial coefficients averaged over the last n iterations
        if FLG_DEBUG == True:
            print("name", self.name)
            print("detected", self.detected)
            print("diffs", self.diffs)
            print("current_fit", self.current_fit)
            print("bestx", self.bestx)
            print("ally", self.ally)
        self.best_fit = np.polyfit(self.ally,self.bestx, 2)
        #p = np.poly1d(self.current_fit)
        #self.best_fit = np.polyfit(self.ally,p(self.ally), 2)
        #z2 = np.polyfit(xp, p(xp), 3)
        #recalc_best_fit = np.polyfit(ploty,left_x_values, 2)
    
    def calc_radius_of_curvature(self):
        self.radius_of_curvature = calc_radius_of_curvature(self.current_fit)

    def calc_diff(self, prev_fit, current_fit):
        self.diffs[0] = abs(current_fit[0] - prev_fit[0])
        self.diffs[1] = abs(current_fit[1] - prev_fit[1])
        self.diffs[2] = abs(current_fit[2] - prev_fit[2])

    #def adjust(self):
    #    self.outlier_fit = self.current_fit
    #    self.current_fit = self.prev_fit
    
    def check_detection(self):
        self.detected = True
        if len(self.recent_xfitted) != 0:
            if self.diffs[0] > self.__FIT_OUTLIER_0 or self.diffs[1] > self.__FIT_OUTLIER_1 or self.diffs[2] > self.__FIT_OUTLIER_2:
                self.detected = False

    def add_recent_data(self, cur_fit, x_values):
        if FLG_DEBUG == True:
            print("current_fit", self.current_fit.shape)
            print("prev_fit", self.prev_fit.shape)

        self.allx = x_values    
        self.calc_diff(self.current_fit, cur_fit)
        #self.detected = True
        #if self.diffs[0] > self.__FIT_OUTLIER_0 or self.diffs[1] > self.__FIT_OUTLIER_1 or self.diffs[2] > self.__FIT_OUTLIER_2:
        #    self.detected = False
        #if self.detected == False:
        #    self.outlier_fit = current_fitx
        #else:
        #    self.prev_fit = self.current_fit
        #    self.current_fit = current_fitx
        #self.adjust()
        
        self.current_fit = cur_fit
        self.check_detection()
        if self.detected == True:
            self.recent_xfitted.append(x_values)
        else:
            self.outlier_fit = self.current_fit
            
        self.calc_radius_of_curvature()
        self.calc_average_x_values_of_fitted_line()
        self.update_averaged_polynomial_coefficients()
        
    def select_fit(self):
        return self.best_fit
        #if self.detected == True:
        #    return self.current_fit
        #else:
        #    return self.best_fit
            


    def text_diffs(self):
        return "{} diff:{:+.4f} {:+.4f} {:+.4f}".format(self.name, self.diffs[0],self.diffs[1],self.diffs[2])
    def text_fits(self): 
        return "{} fits:{:+.4f} {:+.4f} {:+.4f}".format(self.name, self.current_fit[0],self.current_fit[1],self.current_fit[2])
    def text_outlier_fits(self): 
        return "{} out:{:+.4f} {:+.4f} {:+.4f}".format(self.name, self.outlier_fit[0],self.outlier_fit[1],self.outlier_fit[2])
    def text_best_fit(self): 
        return "{} best of last {}:{:+.4f} {:+.4f} {:+.4f}".format(self.name, len(self.recent_xfitted), self.best_fit[0],self.best_fit[1],self.best_fit[2])

    def print_line_info(self):
        if self.name != None:
            print(self.name)
        print(self.text_diffs())
        print(self.text_fits())
        if FLG_DEBUG == True:
            print("diffs", self.diffs.shape)
            print("fits", self.fits.shape)
In [22]:
class Lane():
    def __init__(self):
        self.right_line = Line("R")  
        self.left_line = Line("L")
        #distance in meters of vehicle center from the line
        self.line_base_pos = None

    def calc_radius_of_curvature(self):
        #print(lane.left_line)
        return (self.left_line.radius_of_curvature + self.right_line.radius_of_curvature)/2

    def find_vehicle_position(self):
        self.line_base_pos = find_vehicle_position(self.left_line.current_fit, self.right_line.current_fit)
        return self.line_base_pos
    
    def adjust(self):
        self.right_line.adjust()
        self.left_line.adjust()
    def print_lane_info(self):
        print("[Left Line]")
        self.left_line.print_line_info()
        print("[Right Line]")
        self.right_line.print_line_info()

    def text_radius(self): return "Radius:{:.4f} ".format(self.calc_radius_of_curvature())
    def text_position(self): return "Vehicle Position:{:.4f} ".format(self.find_vehicle_position())
    def text_diffs_left(self): return self.left_line.text_diffs()
    def text_diffs_right(self): return self.right_line.text_diffs()
    def text_fits_left(self): return self.left_line.text_fits()
    def text_fits_right(self): return self.right_line.text_fits()
    def text_outlier_fits_left(self): return self.left_line.text_outlier_fits()
    def text_outlier_fits_right(self): return self.right_line.text_outlier_fits()
    def text_best_fit_left(self): return self.left_line.text_best_fit()
    def text_best_fit_right(self): return self.right_line.text_best_fit()

    def text_alert_msg_left(self):
        msg = ""
        if (self.left_line.detected == False):
            msg = msg + self.text_outlier_fits_left()
        return msg
    def text_alert_msg_right(self):
        msg = ""
        if (self.right_line.detected == False):
            msg = msg + self.text_outlier_fits_right()
        return msg
    def text_alert_msg(self):
        msg = ""
        if (self.left_line.detected == False):
            msg = msg + self.text_outlier_fits_left()
        if (self.right_line.detected == False):
            msg = msg + self.text_outlier_fits_right()
        return msg

    def print_lane_info(self):
        print(self.text_radius())
        print(self.text_position())
        print(self.text_diffs_left())
        print(self.text_diffs_right())
        print(self.text_fits_left())
        print(self.text_fits_right())
        print(self.text_best_fit_left())
        print(self.text_best_fit_right())

Function Definition

In [23]:
# Assuming you have created a warped binary image called "binary_warped"
def find_lane(binary_warped, lane = Lane(), viz=False):

    # Choose the number of sliding windows
    nwindows = 9
    # Set the width of the windows +/- margin
    margin = 100
    #margin = 150
    # Set minimum number of pixels found to recenter window
    minpix = 50
    
    # Take a histogram of the bottom half of the image
    #histogram = np.sum(binary_warped[binary_warped.shape[0]/2:,:], axis=0)
    histogram = np.sum(binary_warped[np.int(binary_warped.shape[0]/2):,:], axis=0)

    if (viz == True) :
        # Create an output image to draw on and  visualize the result
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255

    # Find the peak of the left and right halves of the histogram
    # These will be the starting point for the left and right lines
    midpoint = np.int(histogram.shape[0]/2)
    leftx_base = np.argmax(histogram[:midpoint])
    rightx_base = np.argmax(histogram[midpoint:]) + midpoint

    # Set height of windows
    window_height = np.int(binary_warped.shape[0]/nwindows)
    # Identify the x and y positions of all nonzero pixels in the image
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    # Current positions to be updated for each window
    leftx_current = leftx_base
    rightx_current = rightx_base

    # Create empty lists to receive left and right lane pixel indices
    left_lane_inds = []
    right_lane_inds = []

    # Step through the windows one by one
    right_stop = False
    left_stop = False
    for window in range(nwindows):
        # Identify window boundaries in x and y (and right and left)
        win_y_low = binary_warped.shape[0] - (window+1)*window_height
        win_y_high = binary_warped.shape[0] - window*window_height
        win_xleft_low = leftx_current - margin
        win_xleft_high = leftx_current + margin
        win_xright_low = rightx_current - margin
        win_xright_high = rightx_current + margin
        if (viz == True) :
            # Draw the windows on the visualization image
            cv2.rectangle(out_img,(win_xleft_low,win_y_low),(win_xleft_high,win_y_high),(0,255,0), 2) 
            cv2.rectangle(out_img,(win_xright_low,win_y_low),(win_xright_high,win_y_high),(0,255,0), 2) #(0,0,255), 2) 
        # Identify the nonzero pixels in x and y within the window
        good_left_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
        (nonzerox >= win_xleft_low) &  (nonzerox < win_xleft_high)).nonzero()[0]
        
        good_right_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & 
        (nonzerox >= win_xright_low) &  (nonzerox < win_xright_high)).nonzero()[0]
        # Append these indices to the lists
        ##if left_stop == False:
        left_lane_inds.append(good_left_inds)
        ##if right_stop == False:
        right_lane_inds.append(good_right_inds)
        # If you found > minpix pixels, recenter next window on their mean position
        if len(good_left_inds) > minpix:
            leftx_current = np.int(np.mean(nonzerox[good_left_inds]))
        if len(good_right_inds) > minpix:        
            rightx_current = np.int(np.mean(nonzerox[good_right_inds]))

    # Concatenate the arrays of indices
    left_lane_inds = np.concatenate(left_lane_inds)
    right_lane_inds = np.concatenate(right_lane_inds)

    # Extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds] 
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds] 

    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    
    # Generate x and y values for plotting
    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
    left_x_values = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
    right_x_values = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]

    if lane.left_line.ally == None:
        lane.left_line.ally = ploty
    if lane.right_line.ally == None:
        lane.right_line.ally = ploty   
    lane.left_line.add_recent_data(left_fit, left_x_values)
    lane.right_line.add_recent_data(right_fit, right_x_values)
    
    if (viz == True):
        visualize_lines(binary_warped, left_fit, right_fit, left_lane_inds, right_lane_inds, nonzerox, nonzeroy, out_img)
        lane.print_lane_info()
    if (FLG_DEBUG == True):
        p = np.poly1d(left_fit)
        #recalc_best_fit = np.polyfit(left_x_values[::-1], ploty[::-1], 2)
        recalc_best_fit = np.polyfit(ploty,left_x_values, 2)
        print("L recalc:{:+.4f} {:+.4f} {:+.4f}".format(recalc_best_fit[0],recalc_best_fit[1],recalc_best_fit[2]))
        #print("left x values", left_x_values[::-1])
        #print("left x data", leftx)
        #print("left y data", lefty)
        #print("ploty data", ploty[::-1])
        

    return lane

After it was known where the lines are, in the next frame of video it doesn't need to do a blind search again, but instead it can be fine to just search in a margin around the previous line position.

In [24]:
def find_lane_again(binary_warped, lane, viz=False): 
    
    nonzero = binary_warped.nonzero()
    nonzeroy = np.array(nonzero[0])
    nonzerox = np.array(nonzero[1])
    margin = 100
    
    #margin = 50
    #margin = 30
    
    #left_fit = lane.left_line.current_fit
    #right_fit = lane.right_line.current_fit
    left_fit = lane.left_line.best_fit
    right_fit = lane.right_line.best_fit
    if FLG_DEBUG == True:
        lane.print_lane_info
        print("left_fit",left_fit)
        print("right_fit",right_fit)
    
    left_lane_inds = ((nonzerox > (left_fit[0]*(nonzeroy**2) + left_fit[1]*nonzeroy + 
    left_fit[2] - margin)) & (nonzerox < (left_fit[0]*(nonzeroy**2) + 
    left_fit[1]*nonzeroy + left_fit[2] + margin))) 

    right_lane_inds = ((nonzerox > (right_fit[0]*(nonzeroy**2) + right_fit[1]*nonzeroy + 
    right_fit[2] - margin)) & (nonzerox < (right_fit[0]*(nonzeroy**2) + 
    right_fit[1]*nonzeroy + right_fit[2] + margin)))  

    # Again, extract left and right line pixel positions
    leftx = nonzerox[left_lane_inds]
    lefty = nonzeroy[left_lane_inds] 
    rightx = nonzerox[right_lane_inds]
    righty = nonzeroy[right_lane_inds]
    # Fit a second order polynomial to each
    left_fit = np.polyfit(lefty, leftx, 2)
    right_fit = np.polyfit(righty, rightx, 2)
    # Generate x and y values for plotting
    ploty = np.linspace(0, binary_warped.shape[0]-1, binary_warped.shape[0] )
    left_x_values = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
    right_x_values = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
    
    lane.left_line.add_recent_data(left_fit, left_x_values)
    lane.right_line.add_recent_data(right_fit, right_x_values)
    
    if (viz == True) :
        # Create an output image to draw on and  visualize the result
        out_img = np.dstack((binary_warped, binary_warped, binary_warped))*255
        visualize_lines(binary_warped, left_fit, right_fit, left_lane_inds, right_lane_inds, nonzerox, nonzeroy, out_img)
    if FLG_DEBUG == True:
        lane.print_lane_info()
        
    return lane#left_fit, right_fit, right_line, left_line

Visualization

In [25]:
 def put_lane_info(image, lane):
    cv2.putText(image,lane.text_radius(),(50,100),fontFace = cv2.FONT_ITALIC, fontScale = 3, color = (0,0,255))
    cv2.putText(image,lane.text_position(),(50,200),fontFace = cv2.FONT_ITALIC, fontScale = 3, color = (0,0,255))
    cv2.putText(image,lane.text_diffs_left(),(50,250),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (255,0,0))
    cv2.putText(image,lane.text_diffs_right(),(50,300),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (255,0,0))
    cv2.putText(image,lane.text_fits_left(),(50,350),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (0,255,0))
    cv2.putText(image,lane.text_fits_right(),(50,400),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (0, 255,0))
    cv2.putText(image,lane.text_best_fit_left(),(50,450),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (0,0,255))
    cv2.putText(image,lane.text_best_fit_right(),(50,500),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (0,0,255))
    cv2.putText(image,lane.text_alert_msg_left(),(50,550),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (255,255,255))
    cv2.putText(image,lane.text_alert_msg_right(),(50,600),fontFace = cv2.FONT_ITALIC, fontScale = 1, color = (255,255,255))
    return image
In [26]:
def draw_polylines(img,pts_def):
    pts = np.array(pts_def, np.int32)
    pts = pts.reshape((-1, 1, 2))
    copy = img.copy()
    cv2.polylines(copy, [pts],True,(255,0,0), thickness=2)
    return copy
In [27]:
class LaneProjector():
    def __init__(self, Minv):
        self.Minv = Minv
        
    def project_lane(self, original_image, warped, left_fit, right_fit):
        
        ploty = np.linspace(0, warped.shape[0]-1, warped.shape[0] )
        
        #print("ploty", ploty)
        
        left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
        right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]

        # Create an image to draw the lines on
        warp_zero = np.zeros_like(warped).astype(np.uint8)
        color_warp = np.dstack((warp_zero, warp_zero, warp_zero))

        # Recast the x and y points into usable format for cv2.fillPoly()
        pts_left = np.array([np.transpose(np.vstack([left_fitx, ploty]))])
        pts_right = np.array([np.flipud(np.transpose(np.vstack([right_fitx, ploty])))])
        pts = np.hstack((pts_left, pts_right))

        # Draw the lane onto the warped blank image
        cv2.fillPoly(color_warp, np.int_([pts]), (0,255, 0))

        # Warp the blank back to original image space using inverse perspective matrix (Minv)
        #newwarp = cv2.warpPerspective(color_warp, self.Minv, (image.shape[1], image.shape[0])) 
        newwarp = cv2.warpPerspective(color_warp, self.Minv, (original_image.shape[1], original_image.shape[0])) 
        
        # Combine the result with the original image
        result = cv2.addWeighted(original_image, 1, newwarp, 0.3, 0)
        
        return result
In [28]:
# Generate x and y values for plotting
def visualize_lines(warped, left_fit, right_fit, left_lane_inds, right_lane_inds, nonzerox, nonzeroy, out_img):
    
    ploty = np.linspace(0, warped.shape[0]-1, warped.shape[0] )
    
    out_img[nonzeroy[left_lane_inds], nonzerox[left_lane_inds]] = [255, 0, 0]
    out_img[nonzeroy[right_lane_inds], nonzerox[right_lane_inds]] = [0, 0, 255]
    
    plt.imshow(out_img)
    
    # Generate x and y values for plotting
    left_fitx = left_fit[0]*ploty**2 + left_fit[1]*ploty + left_fit[2]
    right_fitx = right_fit[0]*ploty**2 + right_fit[1]*ploty + right_fit[2]
    plt.plot(left_fitx, ploty, color='yellow')
    plt.plot(right_fitx, ploty, color='yellow')
    
    #print(warped.shape[0], warped.shape[1])

    plt.xlim(0, 1280)
    plt.ylim(720, 0)

Sample Execution

In [29]:
test_img = mpimg.imread('./test_images/test2.jpg')
image_threshed = pipeline_thresh(test_img)

binary_warped, M, Minv, _ = pipeline_warp(image_threshed)

test_lane = find_lane(binary_warped, viz=True)
Radius:3396.5073 
Vehicle Position:-0.3470 
L diff:+0.0002 +0.2648 +76.0237
R diff:+0.0002 +0.1893 +575.6769
L fits:-0.0002 +0.2648 +76.0237
R fits:-0.0002 +0.1893 +575.6769
L best of last 1:-0.0002 +0.2648 +76.0237
R best of last 1:-0.0002 +0.1893 +575.6769
In [30]:
test_img = mpimg.imread('./test_images/test2.jpg')
image_threshed = pipeline_thresh(test_img)

binary_warped, M, Minv, _ = pipeline_warp(image_threshed)

test_lane = find_lane_again(binary_warped, test_lane, viz=True)

Processing Each Image

In [31]:
def show_process_images(image, objpoints, imgpoints, lane=None, sx_thresh=(20, 100)):
    
    undistorted = cal_undistort(image, objpoints, imgpoints)
    thresed = pipeline_thresh(undistorted, sx_thresh=sx_thresh)
    binary_warped, M, Minv, dst = pipeline_warp(thresed)
    undistorted_w_line = draw_polylines(undistorted, SRC_TRAPEZOID)
    thresed_w_line = draw_polylines(undistorted, SRC_TRAPEZOID)
    #binary_warped_w_line = draw_polylines(binary_warped, dst)

    if lane == None:
        lane = find_lane(binary_warped, viz=True)
    else:
        lane = find_lane_again(binary_warped, lane, viz=True)

    left_fit = lane.left_line.current_fit
    right_fit = lane.right_line.current_fit

    lane.print_lane_info()

    lane_projector = LaneProjector(Minv)
    #result = line_projector.project_lines(undistorted_w_line, binary_warped, left_fit, right_fit)
    result = lane_projector.project_lane(undistorted, binary_warped, left_fit, right_fit)

    #show_two_images(thresed, binary_warped_w_line, title1="Threshed Image", title2="Warped Image")
    show_two_images(thresed, binary_warped, title1="Threshed Image", title2="Warped Image")
    
    ploty = np.linspace(0, image.shape[0]-1, image.shape[0] )
    y_eval = np.max(ploty)
    
    #show_two_images(result, result)
    show_two_images(undistorted_w_line, result)

    return lane
In [32]:
test_img = mpimg.imread('./test_images/straight_lines1.jpg')
_ = show_process_images(test_img, objpoints, imgpoints)
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:91: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:93: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
Radius:22266.2814 
Vehicle Position:-0.0805 
L diff:+0.0002 +0.2512 +46.9434
R diff:+0.0002 +0.2357 +38.5222
L fits:+0.0000 +0.0135 +122.9671
R fits:+0.0000 -0.0464 +614.1991
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
Radius:22266.2814 
Vehicle Position:-0.0805 
L diff:+0.0002 +0.2512 +46.9434
R diff:+0.0002 +0.2357 +38.5222
L fits:+0.0000 +0.0135 +122.9671
R fits:+0.0000 -0.0464 +614.1991
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
In [33]:
test_img = mpimg.imread('./test_images/test1.jpg')
_ = show_process_images(test_img, objpoints, imgpoints)
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:91: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:93: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
Radius:8943.3002 
Vehicle Position:-0.1307 
L diff:+0.0001 +0.1721 +71.3573
R diff:+0.0001 +0.0853 +98.0741
L fits:+0.0001 -0.1585 +194.3244
R fits:-0.0000 -0.1316 +712.2732
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
Radius:8943.3002 
Vehicle Position:-0.1307 
L diff:+0.0001 +0.1721 +71.3573
R diff:+0.0001 +0.0853 +98.0741
L fits:+0.0001 -0.1585 +194.3244
R fits:-0.0000 -0.1316 +712.2732
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
In [34]:
test_img = mpimg.imread('./test_images/test2.jpg')
_ = show_process_images(test_img, objpoints, imgpoints)
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:91: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:93: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
Radius:3366.0701 
Vehicle Position:-0.3379 
L diff:+0.0003 +0.4226 +117.3962
R diff:+0.0001 +0.3189 +136.6417
L fits:-0.0002 +0.2640 +76.9281
R fits:-0.0002 +0.1873 +575.6315
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
Radius:3366.0701 
Vehicle Position:-0.3379 
L diff:+0.0003 +0.4226 +117.3962
R diff:+0.0001 +0.3189 +136.6417
L fits:-0.0002 +0.2640 +76.9281
R fits:-0.0002 +0.1873 +575.6315
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
In [35]:
test_img = mpimg.imread('./test_images/test4.jpg')
test_lane = show_process_images(test_img, objpoints, imgpoints)
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:91: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:93: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
Radius:4817.5411 
Vehicle Position:-0.3303 
L diff:+0.0002 +0.3407 +107.8105
R diff:+0.0004 +0.4465 +151.2119
L fits:+0.0001 -0.0767 +184.7387
R fits:+0.0002 -0.2592 +726.8433
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
Radius:4817.5411 
Vehicle Position:-0.3303 
L diff:+0.0002 +0.3407 +107.8105
R diff:+0.0004 +0.4465 +151.2119
L fits:+0.0001 -0.0767 +184.7387
R fits:+0.0002 -0.2592 +726.8433
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769

Check the impact to use the previous result with the image from challenge video

In [36]:
test_img = mpimg.imread('./issue_images/org_video_image_20171217_10_22_06.jpg')
#test_output = pipeline_thresh(test_img, s_thresh=(170, 255), sx_thresh=(5, 100))
_ = show_process_images(test_img, objpoints, imgpoints)#, sx_thresh=(5, 100))
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:91: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
C:\Anaconda3\envs\carnd-term1\lib\site-packages\ipykernel_launcher.py:93: FutureWarning: comparison to `None` will result in an elementwise object comparison in the future.
Radius:1593.1932 
Vehicle Position:0.0031 
L diff:+0.0008 +0.6150 +186.1732
R diff:+0.0001 +0.2102 +150.5462
L fits:+0.0009 -0.6917 +370.9119
R fits:+0.0003 -0.4694 +576.2972
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
Radius:1593.1932 
Vehicle Position:0.0031 
L diff:+0.0008 +0.6150 +186.1732
R diff:+0.0001 +0.2102 +150.5462
L fits:+0.0009 -0.6917 +370.9119
R fits:+0.0003 -0.4694 +576.2972
L best of last 2:-0.0002 +0.2648 +76.0237
R best of last 2:-0.0002 +0.1893 +575.6769
In [37]:
_ = show_process_images(test_img, objpoints, imgpoints,test_lane)
Radius:1104.8506 
Vehicle Position:-0.3025 
L diff:+0.0000 +0.0388 +2.2914
R diff:+0.0008 +0.7293 +11.5121
L fits:+0.0009 -0.7305 +368.6204
R fits:-0.0005 +0.2599 +587.8093
L best of last 3:+0.0002 -0.0670 +173.5559
R best of last 2:-0.0002 +0.1893 +575.6769

Video Generation

In [38]:
DIR_ORG_VIDEO_SHOT = "./video_images/"
DIR_CVT_VIDEO_SHOT = "./video_images/"
PREFIX_VIDEO_FILE_NAME = "video_image_"
SUFFIX_ORG_VIDEO_FILE_NAME = "_org"
SUFFIX_GEN_VIDEO_FILE_NAME = "_gen"

FLG_FILE_OUTPUT = False

ORG_VIDEO_FILE_NAME = "org_video_image_"
CVT_VIDEO_FILE_NAME = "cvt_video_image_"

from PIL import Image
def save_image(image, dirname, filename, suffix= None):
    """save a image file"""  
    datetime_now_text = datetime.now().strftime("%Y%m%d_%H_%M_%S")
    filepath = dirname + filename + datetime.now().strftime("%Y%m%d_%H_%M_%S.jpg")
    if suffix != None:
        filepath = dirname + filename + datetime_now_text + suffix + ".jpg"
    else:
        filepath = dirname + filename + datetime_now_text + ".jpg"
    if not os.path.exists(filepath) :
        Image.fromarray(image).save(filepath)
In [39]:
lane = None
In [40]:
from datetime import datetime
import os

def video_pipeline(image): 
    import __main__
    processed_image = np.copy(image)
    
    # Undistort image 
    processed_image = cal_undistort(processed_image, objpoints, imgpoints)
    
    processed_image = pipeline_thresh(processed_image)
    binary_warped, M, Minv, _ = pipeline_warp(processed_image)

    #if flg_in_process == False:
    if __main__.lane == None:
        __main__.lane = Lane()
        __main__.lane = find_lane(binary_warped, __main__.lane)
    else:
        __main__.lane = find_lane_again(binary_warped, __main__.lane)
    
    left_fit = __main__.lane.left_line.select_fit()#current_fit
    right_fit = __main__.lane.right_line.select_fit()#current_fit

    lane_projector = LaneProjector(Minv)
    processed_image = lane_projector.project_lane(image, binary_warped, left_fit, right_fit)
    processed_image = put_lane_info(processed_image, lane)

    if FLG_FILE_OUTPUT == True:
        save_image(image,DIR_ORG_VIDEO_SHOT, PREFIX_VIDEO_FILE_NAME, SUFFIX_ORG_VIDEO_FILE_NAME)
        save_image(processed_image,DIR_CVT_VIDEO_SHOT, PREFIX_VIDEO_FILE_NAME, SUFFIX_GEN_VIDEO_FILE_NAME)
    
    return processed_image # This must be a color image
In [41]:
def process_image(image):
    # NOTE: The output you return should be a color image (3 channel) for processing video below
    # TODO: put your pipeline here,
    # you should return the final output (image where lines are drawn on lanes)
    result = video_pipeline(image)
    return result
In [42]:
# Import everything needed to edit/save/watch video clips
from moviepy.editor import VideoFileClip
from IPython.display import HTML
In [43]:
CHALLENGE_VIDEO_INPUT = 'challenge_video.mp4'
CHALLENGE_VIDEO_OUTPUT = 'output_images/challenge_video_output.mp4'
FLG_FILE_OUTPUT = True
lane = None
clip1 = VideoFileClip(CHALLENGE_VIDEO_INPUT)
white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(CHALLENGE_VIDEO_OUTPUT, audio=False)
lane = None
[MoviePy] >>>> Building video output_images/challenge_video_output.mp4
[MoviePy] Writing video output_images/challenge_video_output.mp4
100%|████████████████████████████████████████████████████████████████████████████████| 485/485 [06:56<00:00,  1.22it/s]
[MoviePy] Done.
[MoviePy] >>>> Video ready: output_images/challenge_video_output.mp4 

Wall time: 6min 58s
In [44]:
HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(CHALLENGE_VIDEO_OUTPUT))
Out[44]:
In [45]:
VIDEO_INPUT = 'project_video.mp4'
VIDEO_OUTPUT = 'output_images/project_video_output.mp4'
lane = None
## To speed up the testing process you may want to try your pipeline on a shorter subclip of the video
## To do so add .subclip(start_second,end_second) to the end of the line below
## Where start_second and end_second are integer values representing the start and end of the subclip
## You may also uncomment the following line for a subclip of the first 5 seconds
##clip1 = VideoFileClip("test_videos/solidWhiteRight.mp4").subclip(0,5)
clip1 = VideoFileClip(VIDEO_INPUT)
white_clip = clip1.fl_image(process_image) #NOTE: this function expects color images!!
%time white_clip.write_videofile(VIDEO_OUTPUT, audio=False)
lane = None
[MoviePy] >>>> Building video output_images/project_video_output.mp4
[MoviePy] Writing video output_images/project_video_output.mp4
100%|█████████████████████████████████████████████████████████████████████████████▉| 1260/1261 [17:43<00:00,  1.18it/s]
[MoviePy] Done.
[MoviePy] >>>> Video ready: output_images/project_video_output.mp4 

Wall time: 17min 44s
In [46]:
HTML("""
<video width="960" height="540" controls>
  <source src="{0}">
</video>
""".format(VIDEO_OUTPUT))
Out[46]: